
#include "ISO14443p4.h"

#include "CardCpuPrivate.h"
#include "DelayFc.h"
#include <string.h>
#include <stdlib.h>


#define FWT_MAX		FWT_Calc(14)
#define FSD_MaxInf		254

#define Error_Transmit	-1
#define Error_Protocol	-2

enum {
	PROC_OK = 0,
	PROC_ACK = 1,
	PROC_NAK = 2,
	PROC_RT = 3,	//re-transmit
	PROC_ERR = 4,
};

typedef struct
{
	unsigned char pcb;
	unsigned char cid;
	unsigned char nad;
	unsigned char len;
	unsigned char *inf;
}XBlock;

//	static ISO14443p4Param_T clParam;

static unsigned char PCB_Make_I(unsigned char format, unsigned char chain, unsigned char sn) {
	unsigned char pcb = 0x02;

	if (chain) {
		pcb |= 0x10;
	}
	pcb |= sn & 0x01;
	if (format & 0x02) { //CID
		pcb |= 0x08;
	}
	if (format & 0x01) { //NAD
		pcb |= 0x04;
	}
	return pcb;
}

static unsigned char PCB_Make_S(unsigned char format, unsigned char type) {
	unsigned char pcb = 0xC2;

	if (format & 0x02) { //CID
		pcb |= 0x08;
	}
	if (type) { // WTX
		pcb |= 0x30;
	}
	return pcb;
}

static unsigned char PCB_Make_R(unsigned char format, unsigned char nak, unsigned char sn) {
	unsigned char pcb = 0xA2;
	if (format & 0x02) { //CID
		pcb |= 0x08;
	}
	if (nak) {
		pcb |= 0x10;
	}
	pcb |= sn & 0x01;
	return pcb;
}

static unsigned int MaxInfoLength(unsigned char pcb, unsigned int frameSize) {
	unsigned int maxLength = frameSize - 3;
	if (pcb & 0x08) {
		maxLength--;
	}
	if (pcb & 0x04) {
		maxLength--;
	}
	return maxLength;
}

static int Block_IsChain(XBlock* xb) {
	return (xb->pcb & 0x10) != 0;
}

static void Block_Build(XBlock* xb, unsigned char pcb, unsigned char info[], unsigned int ilen) {
	xb->pcb = pcb;
	xb->cid = 0;
	xb->nad = 0;
	xb->inf = info;
	xb->len = (unsigned char)ilen;
}

static unsigned int Block_Build_S(XBlock* xb, ISO14443p4Param_T* param, unsigned char type, unsigned char info[], unsigned int ilen) {
	unsigned char pcb = PCB_Make_S(param->blockFormat, type);
	Block_Build(xb, pcb, info, ilen);
	return ilen;
}

static unsigned int Block_InfoMax(unsigned char format, unsigned int fsc) {
	unsigned int infoMax = fsc - 3;

	if (format & 1) {
		infoMax--;
	}
	if (format & 2) {
		infoMax--;
	}
	return infoMax;
}

static unsigned int Block_Build_I(XBlock* xb, ISO14443p4Param_T* param, unsigned char info[], unsigned int ilen) {
	unsigned int infoMax = Block_InfoMax(param->blockFormat, param->FSC);
	unsigned int xlen = ilen < infoMax ? ilen : infoMax;
	unsigned char chain = infoMax < ilen;
	unsigned char pcb = PCB_Make_I(param->blockFormat, chain, param->sn);

	Block_Build(xb, pcb, info, xlen);
	return xlen;
}

static void Block_Build_R(XBlock* xb, ISO14443p4Param_T* param, unsigned char nak) {
	unsigned char pcb = PCB_Make_R(param->blockFormat, nak, param->sn);
	Block_Build(xb, pcb, 0, 0);
}

static int Block_IsValid(XBlock* xb) {
	return xb->len < 254;
}

static int Block_IsR(XBlock* xb) {
	unsigned char pcb = xb->pcb;
	return (pcb & 0xC0) == 0x80;
}

static int Block_IsACK(XBlock* xb) {
	unsigned char pcb = xb->pcb;
	return (pcb & 0x10) == 0x00;
}

#if 0
static int Block_IsNAK(XBlock* xb) {
	return !Block_IsACK(xb);
}
#endif 

static int Block_IsI(XBlock* xb) {
	unsigned char pcb = xb->pcb;
	return (pcb & 0xC0) == 0x00;
}

static int Block_Verify_R(XBlock* xb) {
	unsigned char pcb = xb->pcb;
	if (((pcb & 0xE0) == 0xA0) && ((pcb & 0x06) == 0x02)) {
		return xb->len == 0;
	}
	return 0;
}

static int Block_Verify_I(XBlock* xb, CpuCard_T* card) {
	unsigned char pcb = xb->pcb;
	(void)card;
	if (((pcb & 0xE0) == 0x00) && ((pcb & 0x02) == 0x02)) {
		return xb->len <= MaxInfoLength(pcb, 256);
	}
	return 0;
}

static int Block_IsWTX(XBlock* xb) {
	unsigned char pcb = xb->pcb;
	return ((pcb & 0xF0) == 0xF0) && ((pcb & 0x07) == 0x02);
}
static int IsValidWTXM(unsigned char wtxm) {
	return (0 < wtxm) && (wtxm < 60);
}
static int Block_Verify_WTX(XBlock* xb) {
	if (Block_IsWTX(xb)) {
		return (xb->len == 1) && IsValidWTXM(xb->inf[0]);
	}
	return 0;
}

static int Block_IsDeselect(XBlock* xb) {
	unsigned char pcb = xb->pcb;
	return (pcb & 0xF0) == 0xC0
		&& (pcb & 0x07) == 0x02;
}

static int Block_Verify_Deslect(XBlock* xb) {
	if (xb->len == 0) {
		return Block_IsDeselect(xb);
	}
	return 0;
}

static int ProtocolCL_VerifyBlock(CpuCard_T* card, XBlock* xb) {
	if (!Block_IsValid(xb)) {
		return 0;
	}
	if (Block_Verify_I(xb, card)) {
		return 1;
	}
	if (Block_Verify_R(xb)) {
		return 1;
	}
	if (Block_Verify_WTX(xb)) {
		return 1;
	}
	if (Block_Verify_Deslect(xb)) {
		return 1;
	}
	return 0;
}

static int Block_GetNumber(XBlock* xb) {
	int sn = xb->pcb & 0x01;
	return sn;
}

#if 0
static unsigned int Block_Unpack(XBlock* xb, unsigned char buf[]) {
	unsigned char pcb = xb->pcb;
	unsigned int len = 0;

	buf[len++] = pcb;
	if (pcb & 0x08) { //CID
		buf[len++] = xb->cid;
	}
	if (pcb & 0x04) { //NAD
		buf[len++] = xb->nad;
	}
	memcpy(buf + len, xb->inf, xb->len);
	len += xb->len;
	return len;
}
#endif 
#if 0

static void Block_Pack(XBlock* xb, unsigned char buf[], unsigned int len) {
	unsigned char pcb = buf[0];
	unsigned int offset = 1;

	xb->pcb = pcb;
	if (pcb & 0x08) { //CID
		xb->cid = buf[offset++];
	}
	if (pcb & 0x04) { //NAD
		xb->nad = buf[offset++];
	}
	if (offset > len) {
		xb->len = 0xFF;
	}
	else {
		if (xb->inf) {
			memcpy(xb->inf, buf + offset, len - offset);
			xb->len = (unsigned char)(len - offset);
		}
	}
}
#endif 


static int Protocol_CL_TxBlock(CpuCard_T* card, XBlock* xb) {
	WCI* wci = card->base.wci;

	int ret = WCI_ClearFifo(wci);
	if (ret < 0) {
		return ret;
	}
	//PCB
	WCI_WriteFifo(wci, &xb->pcb, sizeof(xb->pcb));
	//CID
	if (xb->pcb & 0x08) {
		WCI_WriteFifo(wci, &xb->cid, sizeof(xb->cid));
	}
	//NAD
	if (xb->pcb & 0x04) {
		WCI_WriteFifo(wci, &xb->nad, sizeof(xb->nad));
	}
	//Data
	if (WCI_WriteFifo(wci, xb->inf, xb->len) != xb->len) {
		return -1;
	}
	return 0;
}

static int Protocol_CL_ExchangeCmd(CpuCard_T* card, unsigned int fwt) {
	WCI* wci = card->base.wci;
	ISO14443p4Param_T* param = &card->params;
	WciBits_T wciBits = { 0, 0 };
	WciTime_T xtime;

	WCI_InitTime(&xtime, fwt, param->FGT);
	return WCI_Command(wci, WCI_Cmd_Exchagne, &xtime, &wciBits);
}
static int Protocol_CL_RxBlock(CpuCard_T* card, XBlock* rb, unsigned int rmax) {
	WCI* wci = card->base.wci;

	if (WCI_FifoLength(wci) > rmax) {
		return WCI_S_OverRun;
	}
	//PCB
	WCI_ReadFifo(wci, &rb->pcb, 1);
	//CID
	if (rb->pcb & 0x08) {
		WCI_ReadFifo(wci, &rb->cid, 1);
	}
	//NAD
	if (rb->pcb & 0x04) {
		WCI_ReadFifo(wci, &rb->nad, 1);
	}
	//Data
	rb->len = WCI_FifoLength(wci);
	WCI_ReadFifo(wci, rb->inf, rb->len);
	return 0;
}

static int ProtocolCL_BlockExchange(CpuCard_T* card, XBlock* xb, XBlock* rb, unsigned int rmax, unsigned int fwt) {
	//Send block
	int ret = Protocol_CL_TxBlock(card, xb);
	if (ret) {
		return ret;
	}
	//Execute Command
	ret = Protocol_CL_ExchangeCmd(card, fwt);
	if (ret) {
		return ret;
	}
	//Receive block
	ret = Protocol_CL_RxBlock(card, rb, rmax);
	if (ret) {
		return ret;
	}
	//Verify
	if (!ProtocolCL_VerifyBlock(card, rb)) {
		return Error_Protocol;
	}
	return ret;
}

static int ProtocolCL_WaitTimeExtern(CpuCard_T* card, XBlock* rb, unsigned int fwt) {
	unsigned char wtxm;
	unsigned int wt;
	ISO14443p4Param_T* param = &card->params;
	XBlock xb;

	wtxm = rb->inf[0];
	Block_Build_S(&xb, param, 1, &wtxm, sizeof(wtxm));
	wt = fwt * wtxm;
	if (wt > FWT_MAX) {
		wt = FWT_MAX;
	}
	return ProtocolCL_BlockExchange(card, &xb, rb, FSD_MaxInf, wt);
}

static int ProtocolCL_HandleTransmitError(XBlock* xb) {
	int ret;

	if (Block_IsR(xb)) { //R-Block
		ret = PROC_ACK;
	}
	else {
		ret = PROC_NAK;
	}

	return ret;
}

static int ProtocolCL_Handle_R(XBlock* xb, XBlock* rb) {
	if (Block_IsI(rb)) {
		if (Block_GetNumber(xb) == Block_GetNumber(rb)) {
			return PROC_OK;
		}
	}
	return PROC_ERR;
}

static int ProtocolCL_Handle_Chain(XBlock* xb, XBlock* rb) {
	if (Block_IsACK(rb)) { //ACK
		if (Block_GetNumber(xb) != Block_GetNumber(rb)) {
			return PROC_RT;
		}
		else {
			return PROC_OK;
		}
	}
	//NAK
	return PROC_ERR;
}

static int ProtocolCL_Handle_NonChain(XBlock* xb, XBlock* rb) {
	if (Block_IsI(rb)) { // I-Block
		if (Block_GetNumber(xb) != Block_GetNumber(rb)) {
			return PROC_ERR;
		}
		return PROC_OK;
	}
	//R-Block
	if (Block_IsACK(rb)) { //ACK
		if (Block_GetNumber(xb) != Block_GetNumber(rb)) {
			//follow ISO14443 rule 6, not compatible  with EMV
			return PROC_RT;
		}
	}
	//NAK
	return PROC_ERR;
}

static int ProtocolCL_Handle_I(XBlock* xb, XBlock* rb) {
	int ret;

	if (Block_IsChain(xb)) { //PCD Chaining
		ret = ProtocolCL_Handle_Chain(xb, rb);
	}
	else {
		ret = ProtocolCL_Handle_NonChain(xb, rb);
	}
	return ret;
}

static int ProtocolCL_BlockSession(CpuCard_T* card, XBlock* xb, XBlock* rb, unsigned int rmax, unsigned int fwt) {
	int ret;
	unsigned int c = 0;
	XBlock cb;
	XBlock* tb = xb;
	ISO14443p4Param_T* param = &card->params;

	for (c = 0; c < 3; c++) {
		ret = ProtocolCL_BlockExchange(card, tb, rb, rmax, fwt);
		if ((ret == WCI_S_OK) && Block_IsWTX(rb)) {
			ret = ProtocolCL_WaitTimeExtern(card, rb, fwt);
		}
		if (ret == Error_Protocol) {
			break;
		}
		if (ret == Error_Transmit) {
			ret = ProtocolCL_HandleTransmitError(xb);
		}
		else {
			if (Block_IsR(xb)) {
				ret = ProtocolCL_Handle_R(xb, rb);
			}
			else
				if (Block_IsI(xb)) {
					ret = ProtocolCL_Handle_I(xb, rb);
				}
		}

		if (ret == PROC_OK || ret == PROC_ERR) {
			break;
		}

		if (ret == PROC_ACK) {
			Block_Build_R(&cb, param, 0);
			tb = &cb;
		}
		else
			if (ret == PROC_NAK) {
				Block_Build_R(&cb, param, 1);
				tb = &cb;
			}
			else
				if (ret == PROC_RT) {
					tb = xb;
				}
	}

	return ret;
}

int ISO14443p4_Check(CpuCard_T* card) {
	int ret;
	XBlock tb, rb;
	int rmax = FSD_MaxInf;
	ISO14443p4Param_T* param = &card->params;

	Block_Build_R(&tb, param, 1);
	ret = ProtocolCL_BlockExchange(card, &tb, &rb, rmax, param->FWT);
	return ret != WCI_S_Timeout;
}

int ISO14443p4_Exchange(CpuCard_T * card, unsigned char capdu[], unsigned int clen, unsigned char rapdu[], unsigned int * rlen)
{
	int ret;
	ISO14443p4Param_T* param = &card->params;
	XBlock xb;
	XBlock rb;
	unsigned int txOffset = 0;
	unsigned int rxOffset = 0;

	rb.inf = rapdu;
	do {
		Block_Build_I(&xb, param, capdu + txOffset, clen - txOffset);
		rb.len = 0;
		ret = ProtocolCL_BlockSession(card, &xb, &rb, FSD_MaxInf, param->FWT);
		if (ret) {
			return ret;
		}
		txOffset += xb.len;
		param->sn ^= 1;
	} while (clen > txOffset);

	rxOffset = rb.len;

	while (Block_IsChain(&rb)) {
		Block_Build_R(&xb, param, 0);
		rb.len = 0;
		rb.inf = rapdu + rxOffset;
		ret = ProtocolCL_BlockSession(card, &xb, &rb, FSD_MaxInf, param->FWT);
		if (ret) {
			return ret;
		}
		rxOffset += rb.len;
		param->sn ^= 1;
	}
	*rlen = rxOffset;
	return ret;
}

int ISO14443p4_Deselect(CpuCard_T * card) {
	int ret;
	ISO14443p4Param_T* param = &card->params;
	XBlock xb;
	XBlock rb;
	unsigned char tBuf[2];
	unsigned int i;

	rb.inf = tBuf;
	for (i = 0; i < 2; i++) {
		Block_Build_S(&xb, param, 0, 0, 0);
		ret = ProtocolCL_BlockExchange(card, &xb, &rb, 2, 13560 * 5);
		if (ret == WCI_S_OK) {
			if (Block_IsDeselect(&rb)) {
				break;
			}
			else {
				ret = Error_Protocol;
			}
		}
	}
	return ret;
}

int ISO14443_CheckPresence(CpuCard_T* card){
	XBlock xb, rb;
	ISO14443p4Param_T* param = &card->params;
	int i;
	int ret;
	
	Block_Build_R(&xb, param, 1);
	for(i = 0; i < 2; i++){	
		ret = ProtocolCL_BlockExchange(card, &xb, &rb, 1, 13560 * 5);
		if(ret != WCI_S_Timeout){
			return 1;
		}
	}
	return 0;
}


